package es.uji.viselab.experiment.control;

import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.robot.RobotException;

public class IBVS_Control_loop extends ExperimentControl {

	public IBVS_Control_loop(ExperimentParameters ep) {
		super(ep);
	}

	double[] velocities;

	@Override
	public void cameraFeaturePointsChanged(ImageSource i) {
		
		if (visualServoing != null) {

			double[] velocitiesCameraFrame = visualServoing.compute(i);

			// Velocities are based on camera frame coordinates
			// They should be transformed to world (robot) coordinate system
			velocities = eyeOnHandCameraCoordinatesToRobotCoordinates(velocitiesCameraFrame);

			System.out.println("Displacement end-effector requested: " + velocities[0] + ", "
					+ velocities[1] + ", " + velocities[2]);
		}
	}

	@Override
	public void start() {

		int iterations = 0;
		double error = 9999999;

		velocities = new double[6];
		for (int i = 0; i < 6; i++)
			velocities[i] = 0;
		int maxSteps = ep.getExperiment().getMaxSteps();
		double maxError = ep.getExperiment().getMaxError();
		while (iterations < maxSteps && error > maxError) {
			// System.out.println("Iteration " + iterations + " error: " +
			// error);
			try {
				robot.move(velocities);
			} catch (RobotException e) {
				e.printStackTrace();
			}
			iterations++;

			error = visualServoing.getError();
			System.out.println("Iteration " + iterations + " error: " + error);
		}
		System.out.println("END CONTROL LOOP AT " + iterations + " ITERATIONS ");
	}
}
